using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace HappySprite.Simulation
{
    public static class CircleBoxAlgorithm
    {
        static float[] c = new float[2];
        static float[] bmin = new float[2];
        static float[] bmax = new float[2];
        static float[] dmin = new float[2];
        static float[] cp = new float[2];

        public static bool DetectCollision(CollisionEventArgs e)
        {
            Circle circle = (Circle)e.This.CollisionShape;
            Box box = (Box)e.Other.CollisionShape;

            // Compute coordinates of circle centerpoint in box coordinate system
            Vector2 boxPosition = e.Other.Transform.Position;
            Vector2 circlePosition = e.This.Transform.Position;
            Vector2 p = boxPosition + e.Other.Transform.TransformTransposeWithoutScale(circlePosition - boxPosition);

            float boxWidthRadius = box.HalfExtents.X * e.Other.Transform.Scale.X;
            float boxHeightRadius = box.HalfExtents.Y * e.Other.Transform.Scale.Y;

            float scaleFactor = Math.Max(e.This.Transform.Scale.X, e.This.Transform.Scale.Y);
            float r = circle.Radius * scaleFactor;

            // Project point onto box
            c[0] = cp[0] = p.X;
            c[1] = cp[1] = p.Y;
            bmin[0] = boxPosition.X - boxWidthRadius;
            bmax[0] = boxPosition.X + boxWidthRadius;
            bmin[1] = boxPosition.Y - boxHeightRadius;
            bmax[1] = boxPosition.Y + boxHeightRadius;

            dmin[0] = dmin[1] = 0f;

            for (int i = 0; i < 2; i++)
            {
                if (c[i] < bmin[i])
                {
                    dmin[i] = c[i] - bmin[i];
                    cp[i] = bmin[i]; // Clamp contact point
                }
                else if (c[i] > bmax[i])
                {
                    dmin[i] = c[i] - bmax[i];
                    cp[i] = bmax[i]; // Clamp contact point
                }
            }

            Vector2 d = new Vector2(dmin[0], dmin[1]);
            Vector2 contactPoint = new Vector2(cp[0], cp[1]);

            if (e.CollisionDetails == CollisionDetails.None)
                return d.LengthSquared() <= r * r;

            float dlen = d.Length();
            float depth = r - dlen;

            if (depth <= 0f || dlen == 0f)
                return false;

            e.Normal = -Vector2.Normalize(e.Other.Transform.TransformVectorWithoutScale(d));
            e.Depth = depth;

            // Rotate local contact point to world space
            e.ContactPoint = boxPosition + e.Other.Transform.TransformVectorWithoutScale(contactPoint - boxPosition);

            return true;
        }
    }
}
